#include "gripper.h"
#include <iostream>
#include <unistd.h>
#include <modbus/modbus-rtu.h>

namespace DriverSDK {

Gripper::Gripper(const char* port, int slave_id) : running_(true), slave_id_(slave_id) {
        mb = connect(port, slave_id);
        pthread_mutex_init(&mutex_, nullptr);
}
Gripper::~Gripper() {
    if (mb) {
        modbus_close(mb);
        modbus_free(mb);
    }
    pthread_mutex_destroy(&mutex_);
}

modbus_t* Gripper::connect(const char* port, int slave_id) {
    modbus_t* mb = modbus_new_rtu(port, 115200, 'N', 8, 1);
    if (!mb) return nullptr;

    modbus_set_slave(mb, slave_id);
    modbus_set_response_timeout(mb, 1, 500000);

    if (modbus_connect(mb) == -1) {
        modbus_free(mb);
        return nullptr;
    }

    return mb;
}


bool Gripper::writePosition(int32_t position) {
    uint16_t pos_regs[2] = {
        static_cast<uint16_t>((position >> 16) & 0xFFFF),
        static_cast<uint16_t>(position & 0xFFFF)
    };
    return modbus_write_registers(mb, REG_POS_LOW, 2, pos_regs) == 2;
}

void Gripper::updateStatus() {
    uint16_t pos_regs[2];
    if (modbus_read_registers(mb, REG_REALTIME_POS_LOW, 2, pos_regs) == 2) {
        status_.position = (pos_regs[0] << 16) | pos_regs[1];
        status_.position = ((status_.position-100) * 0.0857142 )*100; //fang

    }

    uint16_t status_reg;
    if (modbus_read_registers(mb, REG_POS_REACHED, 1, &status_reg) == 1) {
        status_.pos_reached = status_reg;
    }
}

void Gripper::setTarget(const GripperTarget& target) {
    pthread_mutex_lock(&mutex_);
    target_ = target;
    pthread_mutex_unlock(&mutex_);
}

void Gripper::getStatus(GripperStatus* status) {
    pthread_mutex_lock(&mutex_);
    *status = status_;
    pthread_mutex_unlock(&mutex_);
}

bool Gripper::control() {
    GripperTarget target;
        pthread_mutex_lock(&mutex_);
        target = target_;
        pthread_mutex_unlock(&mutex_);
    bool write_ok = modbusWrite(REG_ENABLE, 1)
                 && writePosition(target.position)
                 && modbusWrite(REG_SPEED, target.speed)
                 && modbusWrite(REG_TORQUE, target.torque)
                 && modbusWrite(REG_TRIGGER, 1);

    updateStatus();

    return write_ok;
}
bool Gripper::modbusWrite(uint16_t reg, uint16_t value) {
    int retries = 3;
    while (retries-- > 0) {
        if (modbus_write_register(mb, reg, value) == 1) {
            return true;
        }
        std::cerr << "Modbus write failed, retrying..." << std::endl;
        usleep(10);  // 等待100ms再重试
    }
    return false;
}

void GripperController::addGripper(const char* port, int slave_id) {
grippers_.push_back(std::unique_ptr<Gripper>(new Gripper(port, slave_id)));
    // grippers_.push_back(std::make_unique<Gripper>(port, slave_id));
}
int GripperController::run() {
    return pthread_create(&pth_, nullptr, &GripperController::controlThread, this) == 0 ? 0 : -1;
}

void GripperController::stop() {
    running_.store(false);
    if (pth_) {
        pthread_join(pth_, nullptr);
        pth_ = 0;
    }
}

void GripperController::setTarget(int index, const GripperTarget& target) {
    if (index >= 0 && index < static_cast<int>(grippers_.size())) {
        grippers_[index]->setTarget(target);
    }
}

void GripperController::getStatus(int index, GripperStatus* status) {
    if (index >= 0 && index < static_cast<int>(grippers_.size())) {
        grippers_[index]->getStatus(status);
    }
}
void GripperController::controlLoop() {
    constexpr int kControlIntervalUs =  1000000;

    while (running_.load()) {
        for (auto& gripper : grippers_) {
            gripper->control(); // 使用->操作符
            usleep(100000);
        }
        usleep(kControlIntervalUs);
    }
}


void* GripperController::controlThread(void* arg) {
    GripperController* self = static_cast<GripperController*>(arg);
    self->controlLoop();
    return nullptr;
}

} // namespace DriverSDK

